classdef RubidiumParametricMagnetometer < handle
    %RUBIDIUMPARAMETRICMAGNETOMETER Summary of this class goes here
    %   Detailed explanation goes here
    
    properties
        session
        controller
        
        workingPoint
    end
    
    %% Constructor
    methods
        function obj = RubidiumParametricMagnetometer(session, controller)
            %RUBIDIUMPARAMETRICMAGNETOMETER Construct an instance of this class
            %   Detailed explanation goes here
            L = log4m.getLogger();
            
            obj.session = session;
            if nargin == 1
                obj.controller = NMRGyroscope.DeviceController.MainDeviceController(obj.session);
            else
                obj.controller = controller;
            end

            L.trace('RubidiumParametricMagnetometer', 'RubidiumParametricMagnetometer object created.');
        end
    end
    
    %% Public Methods Defined Separatedly
    methods
        gui(obj);
        
        wp                   = GET_WorkingPoint(obj, varargin);
        [amp, err, plt_data] = GET_StdCalibrationSignal(obj, varargin);
        stdSpectrum          = GET_StdCalibrationSpectrum(obj, varargin);
        transField           = GET_TransverseField(obj, varargin);
        [data, slopeY]       = GET_TransverseFieldY_Slope(obj, varargin);
        [data, slopeZ]       = GET_TransverseFieldZ_Slope(obj, varargin)
        dataY                = GET_TransverseFieldY_StepResponse(obj, varargin);
        dataZ                = GET_TransverseFieldZ_StepResponse(obj, varargin);
        
        result = DO_SelfTest(obj);
        result = DO_SweepModulationFreq(obj, frequencyList);
        result = DO_SweepModulationStrength(obj, modulationVoltList);
    end
    
    %% Magnetometer Operations
    methods
        function obj = setMainField(obj)
            obj.controller.setCurrentControlVoltX(); % set main field, use default value
        end
        function freq = getRb85EstFreq(obj)
            volt = obj.controller.getCurrentControlVoltX();
            freq = volt*obj.controller.Conversions.BxDC.Rb85Ratio;
        end
        function freq = getRb87EstFreq(obj)
            volt = obj.controller.getCurrentControlVoltX();
            freq = volt*obj.controller.Conversions.BxDC.Rb87Ratio;
        end
        
        function obj = recoverWorkingPoint(obj)
            if ~isempty(obj.workingPoint)
                obj.controller.setCurrentControlVoltY(obj.workingPoint.biasY);
                obj.controller.setCurrentControlVoltZ(obj.workingPoint.biasZ);
                obj.controller.setMagnetometerDemodPhaseY(obj.workingPoint.phaseY);
                obj.controller.setMagnetometerDemodPhaseZ(obj.workingPoint.phaseZ);
                disp(struct2table(obj.workingPoint));
            else
                error('WorkingPoint not found.');
            end
        end
        %%
        function obj = lockBy(obj)
            obj.controller.lockMagnetometerTransverseFieldY('gain', obj.workingPoint.slopeY, 'center', obj.workingPoint.biasY);
            obj.controller.syncDevice();
        end
        function obj = unLockBy(obj)
            obj.controller.unLockMagnetometerTransverseFieldY();
            obj.controller.syncDevice();
        end
        function obj = lockBz(obj)
            obj.controller.lockMagnetometerTransverseFieldZ('gain', obj.workingPoint.slopeZ, 'center', obj.workingPoint.biasZ);
            obj.controller.syncDevice();
        end
        function obj = unLockBz(obj)
            obj.controller.unLockMagnetometerTransverseFieldZ();
            obj.controller.syncDevice();
        end
        
        function plant = byPlantModel(obj)
            gain = obj.workingPoint.slopeY;
            plant=EquipmentControl.FeedBackOptimization.LTIUnit.StaticGain(gain);
        end
        function filter = byFilterModel(obj)
            order = obj.controller.PIDSettings.ByLock.order; 
            bw = obj.controller.PIDSettings.ByLock.bandwidth;
            filter=EquipmentControl.FeedBackOptimization.LTIUnit.LowPassFilter(order, bw);
        end
        function [pValue, iValue, dValue, tune] = optimal_ByLock(obj)
            tune = EquipmentControl.FeedBackOptimization.PIDOptimizer(obj.byPlantModel, obj.byFilterModel);            
            tune.setControllerOptimal('type', 'PID');
            [pValue, iValue, dValue] = tune.getPIDValue;
        end
        
        function plant = bzPlantModel(obj)
            gain = obj.workingPoint.slopeZ;
            plant=EquipmentControl.FeedBackOptimization.LTIUnit.StaticGain(gain);
        end
        function filter = bzFilterModel(obj)
            order = obj.controller.PIDSettings.BzLock.order; 
            bw = obj.controller.PIDSettings.BzLock.bandwidth;
            filter=EquipmentControl.FeedBackOptimization.LTIUnit.LowPassFilter(order, bw);
        end
        function [pValue, iValue, dValue, tune] = optimal_BzLock(obj)
            tune = EquipmentControl.FeedBackOptimization.PIDOptimizer(obj.bzPlantModel, obj.bzFilterModel);
            tune.setControllerOptimal('type', 'PID');
            [pValue, iValue, dValue] = tune.getPIDValue;
        end
    end
    
    %% Private Methods - GET_WorkingPoint
    methods (Access = {?NMRGyroscope.ParametricMagnetometer.ParametricMagnetometerAPP})
        function [phaseY, phaseZ, biasY, biasZ, stdY, stdZ, sseY, sseZ, slopeY, slopeZ] = scanPhaseBias(obj, varargin)
            p=inputParser;
            levelCheck = @(x) x >= 0.0 && x < 10.0;
            biasRangeCheck = @(x) x > 0.0 && x < 1.0;
            phaseRangeCheck = @(x) length(x) == 2;
            pointCheck = @(x) x > 1 && x <= 200;
            nameCheck = @(x) any(ismember({'RawScan', 'FineScan'}, x));
            p.addParameter('biasY', 1.0, levelCheck);
            p.addParameter('biasZ', 2.0, levelCheck);
            p.addParameter('biasRange', 0.5, biasRangeCheck);
            p.addParameter('biasPoint', 20, pointCheck);
            p.addParameter('phaseYRange', [0 180], phaseRangeCheck);
            p.addParameter('phaseZRange', [0 180], phaseRangeCheck);
            p.addParameter('phasePoint', 50, pointCheck);
            p.addParameter('name', 'RawScan', nameCheck);
            p.parse(varargin{:});
            
            L = log4m.getLogger;
            
            scanName = p.Results.name;
            biasY = p.Results.biasY; 
            biasZ = p.Results.biasZ;
            biasRange = [-p.Results.biasRange, 0.0, p.Results.biasRange];
            nBias = p.Results.biasPoint;
            phaseYMin = p.Results.phaseYRange(1); 
            phaseYMax = p.Results.phaseYRange(2);
            phaseZMin = p.Results.phaseZRange(1); 
            phaseZMax = p.Results.phaseZRange(2);
            nPhase = p.Results.phasePoint;
            
            [phaseY, stdY] = obj.sweepPhaseY(biasY, biasZ + biasRange, phaseYMin, phaseYMax, nPhase);
            [phaseZ, stdZ] = obj.sweepPhaseZ(biasY + biasRange, biasZ, phaseZMin, phaseZMax, nPhase);
            L.info('ParametricMagnetometer::GET_WorkingPoint', sprintf('%s: phaseY = %5.3f, stdY = %5.3f; phaseZ = %5.3f, stdZ = %5.3f', scanName, phaseY, stdY, phaseZ, stdZ));
            
            [biasY, sseY, ~, slopeY] = obj.sweepBiasY(biasY+biasRange(1), biasY+biasRange(end), nBias);
            [biasZ, sseZ, ~, slopeZ] = obj.sweepBiasZ(biasZ+biasRange(1), biasZ+biasRange(end), nBias);
            L.info('ParametricMagnetometer::GET_WorkingPoint', sprintf('%s biasY = %5.3f, sseY = %5.3f; biasZ = %5.3f, sseZ = %5.3f', scanName, biasY, sseY, biasZ, sseZ));
        end
        
        function [optPhase, minStd, dataSweepPhaseY]  = sweepPhaseY(obj, biasY, biasZList, phaseMin, phaseMax, nPhase)
            dataSweepPhaseY = arrayfun(@(biasZ) obj.sweepPhase(obj.controller.DemodSettings.ByDemodSlow.index, biasY, biasZ,...
                                                               'minPhase', phaseMin, 'maxPhase', phaseMax, 'nPhase', nPhase),...
                                                               biasZList);
            [optPhase, minStd] = obj.getOptPhase(dataSweepPhaseY, obj.controller.DemodSettings.ByDemodSlow.index);
            obj.controller.setMagnetometerDemodPhaseY(optPhase);
        end
        function [optPhase, minStd, dataSweepPhaseZ]  = sweepPhaseZ(obj, biasYList, biasZ, phaseMin, phaseMax, nPhase)
            dataSweepPhaseZ = arrayfun(@(biasY) obj.sweepPhase(obj.controller.DemodSettings.BzDemodSlow.index, biasY, biasZ, ...
                                                               'minPhase', phaseMin, 'maxPhase', phaseMax, 'nPhase', nPhase),...
                                                               biasYList);
            [optPhase, minStd]  = obj.getOptPhase(dataSweepPhaseZ, obj.controller.DemodSettings.BzDemodSlow.index);
            obj.controller.setMagnetometerDemodPhaseZ(optPhase);
        end
        function [optBias, sse, dataSweepBiasY, slopeY] = sweepBiasY(obj, minBiasY, maxBiasY, nBias)
            dataSweepBiasY = obj.sweepBias(obj.controller.CurrentSettings.ByCtrlDC.index, minBiasY, maxBiasY, 'nBias', nBias);                        
            data = dataSweepBiasY.getNodeData(obj.controller.DemodSettings.ByDemodSlow.index, 'x');
            [slopeY, optBias, ~, sse] = obj.linearFit(data.grid, data.value);            
            if optBias<0
                optBias=0.0;
                warning('sweepBiasY: negative bias is now allowed. set to zero.');
            end
            obj.controller.setCurrentControlVoltY(optBias);
        end                
        function [optBias, sse, dataSweepBiasZ, slopeZ] = sweepBiasZ(obj, minBiasZ, maxBiasZ, nBias)
            dataSweepBiasZ = obj.sweepBias(obj.controller.CurrentSettings.BzCtrlDC.index, minBiasZ, maxBiasZ, 'nBias', nBias);
            data = dataSweepBiasZ.getNodeData(obj.controller.DemodSettings.BzDemodSlow.index, 'x');
            [slopeZ, optBias, ~, sse] = obj.linearFit(data.grid, data.value);
            if optBias<0
                optBias=0.0;
                warning('sweepBiasZ: negative bias is now allowed. set to zero.');
            end
            obj.controller.setCurrentControlVoltZ(optBias);
        end
        
        function swpr_data = sweepPhase(obj, chSweep, biasY, biasZ, varargin)
            p=inputParser;
            p.addParameter('minPhase', 0.0);
            p.addParameter('maxPhase', 180.0);
            p.addParameter('nPhase', 50);
            p.parse(varargin{:});    

            originalBiasY = obj.controller.getCurrentControlVoltY();
            originalBiasZ = obj.controller.getCurrentControlVoltZ();
            obj.controller.setCurrentControlVoltY(biasY);
            obj.controller.setCurrentControlVoltZ(biasZ);
            
            swpr=obj.controller.ziMaster.sweepModule();

            swpr.setControl('gridnode',  ['demods/' chSweep '/phaseshift'], 'start', p.Results.minPhase, 'stop', p.Results.maxPhase, 'samplecount', p.Results.nPhase);
            swpr.setFilter('bandwidthcontrol', 2); % bandwidth control = 0 (manual) using channel settings in demod panel
            swpr.setSettling('settling_time', 0.0);
            swpr.setStatistics('averaging_sample', 100);

            swpr.reset();
            swpr.subscribeChannel(chSweep, {'x'});
            swpr_data = swpr.run();
            swpr.unsubscribeChannel();

            obj.controller.setCurrentControlVoltY(originalBiasY);
            obj.controller.setCurrentControlVoltZ(originalBiasZ);
        end
        
        function swpr_data = sweepBias(obj, chSweep, minBias, maxBias, varargin)
            p=inputParser;
            p.addParameter('nBias', 20);
            p.addParameter('subscribe', chSweep);
            p.parse(varargin{:}); 
            
            swpr=obj.controller.ziMaster.sweepModule();

            swpr.setControl('gridnode',  ['auxouts/' chSweep '/offset'], 'start', minBias, 'stop', maxBias, 'samplecount', p.Results.nBias);
            swpr.setFilter('bandwidthcontrol', 2); % bandwidth control = 0 (manual) using channel settings in demod panel
            swpr.setSettling('settling_time', 0.0);
            swpr.setStatistics('averaging_sample', 100);

            swpr.reset();
            if iscell(p.Results.subscribe)
                cellfun(@(ch) swpr.subscribeChannel(ch, {'x'}), p.Results.subscribe);
            else
                swpr.subscribeChannel(p.Results.subscribe, {'x'});
            end
            swpr_data = swpr.run();
            swpr.unsubscribeChannel();
        end

        function [slope, intercept_x, intercept_y, sse] = linearFit(~, data_x, data_y)
            [fitRes, gof] = fit(data_x, data_y, 'poly1');
            slope = fitRes.p1;
            intercept_x = -fitRes.p2/fitRes.p1;
            intercept_y = fitRes.p2;
            sse = gof.sse;
        end
        
        function [optPhase, minStd] = getOptPhase(~, data, ch)
            if ~iscell(data)
                data = num2cell(data);
            end

            data_1 = data{1}.getNodeData(ch, 'x');
            phase = data_1.grid;

            val = zeros(length(data), length(phase));
            for k=1:length(data)
                data_k = data{k}.getNodeData(ch, 'x');
                val(k, :) = data_k.value;
            end
            [minStd, idx] = min(std(val));
            optPhase = phase(idx);

            if optPhase > 90.0
                optPhase = optPhase - 180.0;
            end
        end
    end    
end

